/*
 * remote_control.c
 *
 *  Created on: Nov 19, 2021
 *      Author: zz
 */


#include "remote_control.h"
#include <stdio.h>

extern void vPortFree( void *pv );
extern void *pvPortMalloc( size_t xWantedSize );

static struct Button* ps2_btn[PS_BTN_COUNT] = {NULL};
static struct Chassis* chassis = NULL;

static uint8_t up_pressed() {
	return ps2_get_btn(PS_UP);
}

static uint8_t down_pressed() {
	return ps2_get_btn(PS_DOWN);
}

static uint8_t left_pressed() {
	return ps2_get_btn(PS_LEFT);
}

static uint8_t right_pressed() {
	return ps2_get_btn(PS_RIGHT);
}

static uint8_t cross_pressed() {
	return ps2_get_btn(PS_CROSS);
}

static uint8_t delta_pressed() {
	return ps2_get_btn(PS_DELTA);
}

/**
 * 按键处理函数, 长按变化1， 短按变化10；  中间预留2的过渡带。
 * up回正、down停车回正、left方向盘左、right方向盘向右，  三角(右上):前进+/后退- 交叉(右下)：前进-/后退+
 */
static void  pressed_handle(void *btn) {
	int delta = 1;
	if(((struct Button*)btn)->event == PRESS_DOWN) {
		delta = 10;
	}
	else if(((struct Button*)btn)->event == LONG_PRESS_HOLD){
		delta = 1;
	}

	if(btn == ps2_btn[PS_UP]) {
		chassis->steering_percent = 0;
		printf("steering reset\r\n");
	}
	else if(btn == ps2_btn[PS_DOWN]) {
		chassis->steering_percent = 0;
		chassis->throttle_percentage = 0;
		printf("stop chassis\r\n");
	}
	else if(btn == ps2_btn[PS_RIGHT]) {
		chassis->steering_percent += delta;
		if(chassis->steering_percent > 100)
			chassis->steering_percent = 100;

		printf("steering_percent: %d\r\n", (int)chassis->steering_percent);
	}
	else if(btn == ps2_btn[PS_LEFT]) {
		chassis->steering_percent -= delta;
		if(chassis->steering_percent < -100)
			chassis->steering_percent = -100;

		printf("steering_percent: %d\r\n", (int)chassis->steering_percent);
	}
	else if(btn == ps2_btn[PS_DELTA]) {
		chassis->throttle_percentage += delta;

		if(chassis->throttle_percentage > 100)
			chassis->throttle_percentage = 100;

		if(chassis->throttle_percentage >= -2 && chassis->throttle_percentage <= 2)
			chassis->gear = GEAR_NEUTRAL;
		else if(chassis->throttle_percentage > 2)
			chassis->gear = GEAR_DRIVE;

		printf("throttle_percentage: %d, gear: %d\r\n", chassis->throttle_percentage, chassis->gear);
	}
	else if(btn == ps2_btn[PS_CROSS]) {
		chassis->throttle_percentage -= delta;
		if(chassis->throttle_percentage < -100)
			chassis->throttle_percentage = -100;

		if(chassis->throttle_percentage >= -2 && chassis->throttle_percentage <= 2)
			chassis->gear = GEAR_NEUTRAL;
		else if(chassis->throttle_percentage < -2)
			chassis->gear = GEAR_REVERSE;

		printf("throttle_percentage: %d, gear: %d\r\n", chassis->throttle_percentage, chassis->gear);
	}
}

void remote_ctrl_start()
{
	//分配底盘资源
	if(!chassis) {
		chassis = (struct Chassis*)pvPortMalloc(sizeof(struct Chassis));
	}
	chassis->gear = GEAR_NEUTRAL;
	chassis->throttle_percentage = 0;
	chassis->rpm = 0;
	chassis->steering_percent = 0;

	chassis->throttle_ratio = 15;  //测试校准值
	chassis->throttle_offset = 0;

	chassis->steering_ratio = 3.5;  //测试校准值
	chassis->steering_offset = 1500;

	//分配按键资源  方向键、三角->回正 交叉->停车
	if(!ps2_btn[PS_UP]) {
		ps2_btn[PS_UP] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	if(!ps2_btn[PS_DOWN]) {
		ps2_btn[PS_DOWN] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	if(!ps2_btn[PS_RIGHT]) {
		ps2_btn[PS_RIGHT] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	if(!ps2_btn[PS_LEFT]) {
		ps2_btn[PS_LEFT] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	if(!ps2_btn[PS_DELTA]) {
		ps2_btn[PS_DELTA] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	if(!ps2_btn[PS_CROSS]) {
		ps2_btn[PS_CROSS] = (struct Button*)pvPortMalloc(sizeof(struct Button));
	}

	//添加按键信息至中间件中
	button_init(ps2_btn[PS_UP], up_pressed, 1);
	button_init(ps2_btn[PS_DOWN], down_pressed, 1);
	button_init(ps2_btn[PS_RIGHT], right_pressed, 1);
	button_init(ps2_btn[PS_LEFT], left_pressed, 1);
	button_init(ps2_btn[PS_DELTA], delta_pressed, 1);
	button_init(ps2_btn[PS_CROSS], cross_pressed, 1);

	//短按事件注册
	button_attach(ps2_btn[PS_UP], 		PRESS_DOWN,       pressed_handle);
	button_attach(ps2_btn[PS_DOWN], 	PRESS_DOWN,       pressed_handle);
	button_attach(ps2_btn[PS_RIGHT], 	PRESS_DOWN,       pressed_handle);
	button_attach(ps2_btn[PS_LEFT], 	PRESS_DOWN,       pressed_handle);
	button_attach(ps2_btn[PS_DELTA], 	PRESS_DOWN,       pressed_handle);
	button_attach(ps2_btn[PS_CROSS], 	PRESS_DOWN,       pressed_handle);

	//长按事件注册
	button_attach(ps2_btn[PS_DELTA],	LONG_PRESS_HOLD,       pressed_handle);
	button_attach(ps2_btn[PS_CROSS], 	LONG_PRESS_HOLD,       pressed_handle);
	button_attach(ps2_btn[PS_RIGHT], 	LONG_PRESS_HOLD,       pressed_handle);
	button_attach(ps2_btn[PS_LEFT], 	LONG_PRESS_HOLD,       pressed_handle);

	//开始处理按键事件
	button_start(ps2_btn[PS_UP]);
	button_start(ps2_btn[PS_DOWN]);
	button_start(ps2_btn[PS_RIGHT]);
	button_start(ps2_btn[PS_LEFT]);
	button_start(ps2_btn[PS_DELTA]);
	button_start(ps2_btn[PS_CROSS]);
}

void remote_ctrl_proc()
{
	int mot_value = 0;
	if(chassis->gear==GEAR_DRIVE || chassis->gear==GEAR_REVERSE)
		mot_value = (int)(chassis->throttle_ratio*chassis->throttle_percentage + chassis->throttle_offset);

	mot_set(0, mot_value);
	mot_set(1, mot_value);

	int servo_value = (int)(chassis->steering_ratio*chassis->steering_percent + chassis->steering_offset);
	servo_set(0, servo_value);
}

void remote_ctrl_exit()
{
	if(chassis) {
		vPortFree(chassis); chassis = NULL;
	}

	if(ps2_btn[PS_UP]) {
		button_stop(ps2_btn[PS_UP]);
		vPortFree(ps2_btn[PS_UP]); ps2_btn[PS_UP] = NULL;
	}
	if(ps2_btn[PS_DOWN]) {
		button_stop(ps2_btn[PS_DOWN]);
		vPortFree(ps2_btn[PS_DOWN]); ps2_btn[PS_DOWN] = NULL;
	}
	if(ps2_btn[PS_RIGHT]) {
		button_stop(ps2_btn[PS_RIGHT]);
		vPortFree(ps2_btn[PS_RIGHT]); ps2_btn[PS_RIGHT] = NULL;
	}
	if(ps2_btn[PS_LEFT]) {
		button_stop(ps2_btn[PS_LEFT]);
		vPortFree(ps2_btn[PS_LEFT]); ps2_btn[PS_LEFT] = NULL;
	}

	if(ps2_btn[PS_DELTA]) {
		button_stop(ps2_btn[PS_DELTA]);
		vPortFree(ps2_btn[PS_DELTA]); ps2_btn[PS_DELTA] = NULL;
	}

	if(ps2_btn[PS_CROSS]) {
		button_stop(ps2_btn[PS_CROSS]);
		vPortFree(ps2_btn[PS_CROSS]); ps2_btn[PS_CROSS] = NULL;
	}
}
